#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>

class OdomTracker {
public:
    OdomTracker() {
        // 初始化ROS节点
        ros::NodeHandle nh;

        // 初始化订阅和发布的话题
        odom_sub_ = nh.subscribe("/odom", 10, &OdomTracker::odomCallback, this);
        cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

        // 设置默认的机器人位姿和速度
        current_pose_.pose.pose.position.x = 0.0;
        current_pose_.pose.pose.position.y = 0.0;
        current_pose_.pose.pose.orientation.w = 1.0;
        current_velocity_.linear.x = 0.0;
        current_velocity_.angular.z = 0.0;

        // 初始化目标距离为1米
        target_distance_ = 1.0;
    }

private:
    ros::Subscriber odom_sub_;
    ros::Publisher cmd_vel_pub_;
    nav_msgs::Odometry current_pose_;
    geometry_msgs::Twist current_velocity_;
    double target_distance_;
    double distance_traveled_;

    void odomCallback(const nav_msgs::Odometry::ConstPtr &msg) {
        // 更新机器人的位姿信息
        current_pose_ = *msg;


        // 计算机器人已走过的距离
        distance_traveled_ = sqrt(current_pose_.pose.pose.position.x * current_pose_.pose.pose.position.x +
                                  current_pose_.pose.pose.position.y * current_pose_.pose.pose.position.y);
        ROS_INFO("Distance traveled: %f", distance_traveled_);

        if (distance_traveled_ < target_distance_) {
            // 如果机器人还没有走完目标距离，则继续前进
            current_velocity_.linear.x = 0.1;
            current_velocity_.angular.z = 0.0;
            cmd_vel_pub_.publish(current_velocity_);
        } else {
            // 如果机器人已经走完目标距离，则停止机器人
            current_velocity_.linear.x = 0.0;
            current_velocity_.angular.z = 0.0;
            cmd_vel_pub_.publish(current_velocity_);
            ROS_INFO("Target distance reached");
        }
    }
};

int main(int argc, char **argv) {
    // 初始化ROS节点
    ros::init(argc, argv, "cre_odom_tracker_distance");

    // 创建OdomTracker对象
    OdomTracker tracker;

    // 进入ROS循环
    ros::spin();

    return 0;
}